//
// Created by vivi on 12/07/2017.
//
#ifndef HAIRCORE_SDF_H
#define HAIRCORE_SDF_H

#include <string>
#include <Eigen/Eigen>

//Signed distance field object
namespace XRwy {

    class SDFObject {
    public:
        SDFObject(const std::string file_path, const Eigen::Affine3f world2local, const float collision_tolerance_, const float impulse_time_);
        SDFObject(SDFObject && rhs);

        void fix(Eigen::Vector3f &pos, Eigen::Vector3f &vel, const float t_);

        void update(const Eigen::Affine3f world2local, const float t_) {
            assert(t_ > 0);
            this->prev_world2local = this->current_world2local;
            this->current_world2local = world2local;

            this->pos2world_vel = (1.0f / t_) * (current_world2local.inverse(Eigen::Affine).matrix() - prev_world2local.inverse(Eigen::Affine).matrix()).topLeftCorner(3, 3);
            this->pos2model_vel = current_world2local.matrix().topLeftCorner(3, 3) * this->pos2world_vel;

            current_world2local_mat3x3 = current_world2local.matrix().topLeftCorner(3, 3);
            current_local2world = current_world2local.inverse(Eigen::Affine);
            current_local2world_mat3x3 = current_local2world.matrix().topLeftCorner(3, 3);
        }

    private:
        inline float query(const float x, const float y, const float z, Eigen::Vector3f *grad = nullptr) const;

        inline float get_pos(int ix, int iy, int iz) const {
            return data[iz * nxy + iy * nx + ix];
        }

        inline float get_target_dist(float dist) const {
            //return dist_tol_div_5 * log(1 + exp(dist_tol_div_5_1 * dist));
	        return dist_tol;
        }

        Eigen::Affine3f current_world2local; //current world to local transformation
        Eigen::Matrix3f current_world2local_mat3x3;
        Eigen::Affine3f current_local2world;
        Eigen::Matrix3f current_local2world_mat3x3;

        Eigen::Affine3f prev_world2local; //previous world to local transformation
        Eigen::Matrix3f pos2world_vel; //get world velocity from position in model(matrix)
        Eigen::Matrix3f pos2model_vel; //get model velocity from position in model(matrix)

        float dist_tol; //the distance of the tolerance
        float dist_tol_div_5; //dist_tol / 5.0
        float dist_tol_div_5_1; //1.0f / dist_tol_div_5

        float impulse_time; //the time for the velocity impulse

        int nx, ny, nz, nxy;
        float origin_x, origin_y, origin_z;
        float len_x;
        float len_y;
        float len_z;
        float len_diagonal;
        float dx;
        float *data;
    };
}

#endif //HAIRCORE_SDF_H
